#include "Communication/SubObjectPoseArray.h"
#include "modules/CtrlModule/ActivePlanner/CliffFollowPlanner.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
    ObjectData DataSet::object_data;
    namespace SubObjectPoseArray
    {
        
        static rclcpp::Subscription< sweeping_robot_msg::msg::ObjectPositionArray>::SharedPtr objectSub;
        void sub_callback(const sweeping_robot_msg::msg::ObjectPositionArray::SharedPtr msg)
        {
            std::vector<PointF> poses_data;
            
            for(int i=0; i< msg->object_pose_array.polygon.points.size(); i++)
            {
                poses_data.push_back(PointF( msg->object_pose_array.polygon.points[i].x, msg->object_pose_array.polygon.points[i].y));
            }
            DataSet::object_data.SetObjectData(poses_data,msg->pushable,msg->object_pose_array.header.stamp.sec,msg->object_pose_array.header.stamp.nanosec);
        }

        void SubObjectPoseArrayInfo(rclcpp::Node::SharedPtr &node_handle)
        {
            objectSub = node_handle->create_subscription<sweeping_robot_msg::msg::ObjectPositionArray>(
                "object_info", rclcpp::SensorDataQoS(), sub_callback);
            std::cout << "SubObjectPoseArray SubObjectPoseArrayInfo" << std::endl;
        }
    } // namespace SubObjectPoseArray

} // namespace behavior_controller